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DESCRIPTION 
FLOATING MOBILE OBJECT CONTROL SYSTEM 

TECHNICAL FIELD 

[0001] The present invention relates to control systems for 
floating mobile objects in water, air, outer space, etc., or mobile 
objects for sliding on a plane. 

BACKGROUND ART 

[0002] First, a prior Icnown floating mobile object control 
technique is described with reference to FIGS. 11 through 14 and 
so on. FIG. 11 is a schematic diagram of a prior Jcnown floating 
mobile object, and FIGS. 11 through 14 are schematic diagrams 
illustrating a prior Icnown floating mobile object control 
technique. 

[0003] As shown in FIG. 11, in order to control the position and 
velocity of the main body of a floating mobile object 10 such as 
an underwater robot, the main body is provided with a sensor 13 
by which the position and velocity of the main body of the floating 
mobile object 10 are sensed and fed to a subtraction circuit 11 
in a computer P. The subtraction circuit 11 is also fed with a 
signal indicating desired position and velocity from an operator 
H. An output of the subtraction circuit 11 is provided to a thruster 
control circuit 12 for controlling thrusters T provided as thrust 
generation means (effectors) on the floating mobile object 10, 



thereby controlling the position and velocity of the floating 
mobile object 10 (which is what is called position/velocity 
feedback control) . 

[0004] FIG. 12 is a block diagram for controlling the floating 
mobile object 10 shown in FIG. 11, An effector thrust command 
signal, which is an output of the thruster control circuit 12, 
is fed to the thrusters T having a predetermined thrust 
characteristic 14, thereby feeding an effector thrust to the 
floating mobile object 10, so that dynamics of the floating mobile 
object 10, i.e., dynamic input/output characteristics, are 
obtained as denoted by reference character 15, and the position 
and velocity of the main body is sensed by the sensor 13 and fed 
to the subtraction circuit 11 as described above. 
[0005] As such, in the conventional control of a floating mobile 
object, information concerning the position and velocity of a 
floating mobile object, which is a subject of control, is f ed bacJc 
to directly handle a thrust command to an effector, and in the 
case of a floating mobile object generally having an effector with 
a relatively slow response speed, it taj^es a time while after the 
command is sent to the effector, the effect of which is reflected 
in the position and velocity of the floating mobile object, until 
the sensor obtains them, therefore there arises a problem where 
control performance is adversely affected (which is what is called 
a sensing delay problem) . This tendency becomes noticeable as 
the mass of the floating mobile object is increased or the thrust 



of the effector is decreased. 

Further, a floating mobile object in a stream of water or 
air is always affected by hydrodynamic disturbances such as waves 
and tidal current or wind, and in addition, as shown in FIG. 11, 
the floatingmobile object 10 in water is connectedwi than umbilical 
cable C (a feeder, a communication line or the like) from above 
the water, which often pulls the floating mobile object 10, 
therefore in the case of the above-described control technique, 
a response to unknown disturbances takes time, causing a problem 
where control performance is adversely affected. 
[0006] Next, in the case where the floating mobile object 10 is 
further provided with a robot arm A having a plurality of links 
as shown in FIG. 11, a block diagram as illustrated in FIG. 13 
is constructed. A reaction force and torque by the robot arm A 
as denoted by reference character 16 is added to effector thrusts 
by the thrusters T as illustrated by an adding circuit 17. 
Accordingly, the reaction force and torque from the robot arm A 
disturbs the action of the floating mobile object 10, i.e., the 
position and velocity, and the thrusters T are controlled to correct 
errors in the position and velocity of the floating mobile object 
10 that are caused by the reaction force and torque of the robot 
arm A. As such, the action (position and velocity) of the floating 
mobile object 10 is disturbed by the reaction force and torque 
from the robot arm A, and therefore conventional attitude control 
activates the thrusters T so as to correct the resulting errors 



4 



A 

« • 

in the position and velocity. 

[0007] Specifically, the prior known control technique operates 
activates the thrusters T after the floating mobile object 10 is 
moved by the reaction force and torque of the robot arm A, and 
5 therefore the so-called sensing delay problem where a response 
speed to disturbances is low still remains unsolved. 
[0008] FIG. 14 illustrates a bloclc diagram of another conventional 
art. This control teclinique talces into consideration mutual 
interference between the motion of the robot arm A and the motion 

10 of the main body of the floatingmobile object 10, and simultaneously 
determines an effector thrust and a robot arm joint torque, such 
that their motions are consistent with their respective purposes. 
In this conventional art, the floating mobile object 10 provided 
with the robot arm A is configured as a multi-linlc system, and 

15 based on its equation of motion, influence of each joint torque 
of each robot arm on the motion of the main body of the floating 
mobile object 10 is expressed in a mathematical formula, which 
is solved to determine propulsion power of the main body of the 
floating mobile object 10 (which is what is called model-based 

20 control) . 

[0009] In the conventional art of FIG. 14, an effector thrust 
command signal from a thruster/robot arm control circuit 18 is 
fed to the thrusters T, and a torque command signal of the robot 
arm A is fed to a j oint actuator 19 . As such, complex system dynamics 
25 20 of the main body of the floating mobile object 10 and the robot 



arm A are formed by effector thrusts outputted from the thrusters 
T and a joint torque by the joint actuator 19, and the sensor 13 
senses the position and velocity of the main body of the floating 
mobile object 10 and feeds them to the subtracter 11 to which a 
signal representing desired position and attitude of the main body 
of the floating mobile object 10 is fed. Also, robot arm control 
variables from the complex system dynamics 20, i.e., signals 
representing the position, attitude, joint angle, velocity, hand 
reaction force, etc., are fed to the subtraction circuit 21 to 
which target values for the control variables of the robot arm 
A are fed. Outputs of the subtracters 11 and 21 are fed to the 
thruster/robot arm control circuit 18. 

Specifically, this control technique takes into 
consideration the mutual interference between the motion of the 
robot arm A and the motion of the main body of the floating mobile 
object 10, and simultaneously determines an effector thrust and 
a robot arm joint torque, such that their motions are consistent 
with their respective purposes . While this advantageously allows 
the motion of the main body of the floating mobile object 10 to 
be accurately controlled, there are the following problems (1) 
through (3 ) . 

[0010] (1) The object dynamics are generally complicated, and if 
the robot arm A has six axes, the dynamics take a form of a link 
joining seven rigid bodies including the main body of the floating 
mobile object 10 . In the case where there are a plurality of robot 
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arms A, the dynamics become more significantly complicated. 
Arithmetic of such control system is considerably time-consuming, 
and imposes a heavy computation load on the computer. Therefore, 
it is practically very difficult to configure the above-described 
5 control system with a small computer, 

[0011] (2) Also, the above-described control system requires a 
number of dynamic parameters such as the mass, moment of inertia 
and gravitational center position of each link of the robot arm 
A and the main body, and proper control cannot be realized by using 

10 these dynamic parameters unless their values are all correct. 
Accordingly, in the case of holding an object with the robot arm 
A, the mass, moment of inertia and gravitational center position 
of the held object should be provided appropriately. Thus, it 
is required to have a database of ob j ects that can be held or measure 

15 dynamic characteristics of the object, therefore there arises a 
problem where the use is practically limited. 

(3) Further, a floating mobile object in a stream of water 
or air is always affected by unknown hydrodynamic disturbances 
such as waves and tidal current or wind, and in addition, as shown 

20 in FIG. 11, the floating mobile object 10 in water is connected 
with an umbilical cable C (a feeder, a communication line or the 
like) from above the water, which often pulls the floating mobile 
object 10, besides it is not possible to model the unknown 
disturbances, and in addition it is difficult to predict influence 

25 of the disturbances, therefore suchamodel-basedcontrol technique 



is disadvantageous in that it cannot be fundamentally adapted to 
control of a floating mobile object. 

[0012] Other prior known floatingmobile object control techniques 
include, for example, feedback control of force at a joint of a 
5 manipulator as disclosed in Patent Dociament 1 and local feedback 
control of a thrust as disclosed in Non-patent Document 1. 
[0013] According to the feedback control of a force at a joint 
of a manipulator in Patent Document 1, it is possible to activate 
thrusters before the main body of a floating mobile object is moved 

10 due to disturbances by the manipulator, thereby canceling any 
influence on the main body of the floating mobile object and 
preventing the main body of the floating mobile object from being 
moved by the disturbances by the manipulator. 
[0014] Also, the thrusters used as effectors for controlling the 

15 position/velocity of the floatingmobile object generates a thrust 
by taking advantage of a stream, and therefore a high nonlinearity 
caused by the stream is present between an input command to the 
thrusters and a thrust that is actually outputted. Such a 
nonlinearity results from various factors, and conventionally it 

20 is difficult to perform control with consideration of all of them 
(a problem of what is called a nonlinearity of an effector dynamic 
characteristic) . However, according to the local feedback 
control of a thrust in Non-Patent Document 1, it is possible to 
prevent control performance for the floating mobile object from 

25 deteriorating due to the nonlinearity of effector dynamic 
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characteristic . 

[0015] While both of the above-described control techniques 
according to Patent Document 1 and Non-Patent Document 1 measure 
and feed back force partially, they do not cancel influence of 
waves and tidal current or wind which are the greatest disturbances 
to the floating mobile object. 

[0016] In brief, neither of the above-described conventional arts 
realizes a floating mobile object control system capable of 
automatically compensating for disturbing influence even under 
disturbances caused by reaction, etc . , of a robot arm and influence 
of unlcnown hydrodynamic disturbances such as waves and tidal 
current or wind to which a floating mobile object in water, air 
or the like is always subjected, thereby allowing the floating 
mobile object to stand still with high precision or traclc a target 
trajectory with high precision. 

[0017] On the other hand, aside from the above-described problems , 
there is a below-described problem with inertial navigation, which 
is a prior known technique widely used for navigating a floating 
mobile object. Specifically, the inertial navigation uses an 
acceleration obtained from a floating mobile object, and in the 
case where the floating mobile object itself is large and heavy, 
a prior known acceleration sensor, which temporally converts an 
acceleration to a displacement and outputs the value of 
acceleration as an electric signal, is structurally unable to sense 
a minor change in the acceleration with high precision, therefore 



there arises a problem where a noticeable error is present between 
the acceleration value obtained from the sensor and an actual 
acceleration value . Such an error between the acceleration values 
considerably affects the accuracy of the inertial navigation itself , 
5 causing inconvenience of being unable to accurately guide the 
floating mobile object onto a desired trajectory. 

[Patent Document 1] Japanese Laid-Open Patent Publication 
No. 5-119837 

[Non-Patent Document 1] Kanaoka, Nakayama, Hayashi and 
10 Kawamura, "Thrust Local Feed-Back Control for Realization of High 
Speed and Precise Motion in Water", proceedings of the Robotics 
and Mechatronics Conference '03, the Japan Society of Mechanical 
Engineers, 2P1-2F-A6, 2003 

15 DISCLOSURE OF THE INVENTION 

PROBLEMS TO BE SOLVED BY THE INVENTION 

[0018] Therefore, an objective of the present invention is to 
provide a floating mobile object control system capable of causing 
a floating mobile object to stand still in a predetermined position 

20 withhigh precision or tracka target trajectory withhigh precision, 
even under disturbances caused by, for example, waves and tidal 
current or wind or disturbances caused by reaction, etc., of a 
robot arm attached to the floating mobile object. 
[0019] In addition, an objective of the present invention is to 

25 provide a floatingmobile object control systemhaving a relatively 
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simple structure and capable of accurately control ling the position, 
attitude, etc, of a floating mobile object, 

[0020] Further, an objective of the present invention is to provide 
a floating mobile object acceleration sensing system capable of 
5 sensing an acceleration of a floating mobile object with high 
precision to such an extent that a minor change in acceleration 
can be sensed, regardless of size and weight of the floating mobile 
object or without depending on the capability of an acceleration 
sensor. 

10 

SOLUTION TO THE PROBLEMS 

[0021] The inventor of the present invention completed the 
invention based on findings that by dividing a floating mobile 
object into a main body part that can be considered as a single 

15 rigid body constituting a part of the floating mobile object and 
an effector part for generating a thrust for the floating mobile 
object, and providing a force/ torque sensor at an only portion 
(a thrust transfer gate) that dynamically connects the main body 
part and the effector part, it is made possible to measure all 

20 forces and torques applied between the main body part and the 
effector part, because a value sensed by the force/ torque sensor 
is a thrust fed from the effector part to the main body part, and 
directly control a thrust that is to be fed to the floating mobile 
object by feeding back the value, whereby it is possible to allow 

25 the floating mobile object to stand still in a predetermined 
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position with high precision, or track a target trajectory with 
high precision, even under hydrodynainic disturbances caused by 
waves, tidal current, etc. 

[0022] A control system for a floating mobile object of the present 
5 invention that can solve the above objectives is (1) a control 
system for a floating mobile object, in which the floating mobile 
object includes: amain body part that can be considered as a single 
rigid body constituting a part of the floating mobile object; an 
effector part for generating a thrust for the floating mobile 

10 object; and a thrust transfer gate for dynamically connecting the 
main body part and the effector part, the thrust transfer gate 
being adapted to be able to actually measure a thrust from the 
effector part acting on the main body part, and a measured value 
for the thrust from the thrust transfer gate is used to obtain 

15 a thrust command to the effector part. 

[0023] Further, in the control system for a floating mobile object 
according to the present invention, (2) the main body part includes 
acceleration measurement means capable of measuring an 
acceleration of the main body part, the thrust transfer gate 

20 includes force/torque measurement means capable of measuring a 
force/torque applied between the main body part and the effector 
part, and the thrust command to the effector part is obtained by 
using: a target acceleration trajectory command obtained by 
converting a target position/velocity trajectory command from an 

25 operator; an output from the acceleration measurement means of 
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the main body part; and an output from the force/ torque measurement 
means from the thrust transfer gate. 

[0024] Note that in the case of applying a control system and an 
acceleration sensing system of the present invention to a floating 
5 mobile object for sliding on a slope as well as a floating mobile 
object for floating in three-dimensional space, which will be 
described in each embodiment and example below, it is necessary 
to compensate for influence of a gravitational acceleration, 
therefore it is required to provide inclination angle measurement 

10 means capable of measuring an inclination of the main body part 
as defined by the invention according to (4) or (8) below. 

However, when applying the control system or acceleration 
sensing system of the present invention to a floating mobile object 
for floating in outer space where basically no gravity is applied 

15 or a floating mobile object for sliding on a horizontal plane for 
which it is not necessary to consider any inclination or it is 
possible to ignore any inclination, the inclination angle 
measurement means as defined by the invention according to the 
above (2), the next-described (3) or the later-described (7) is 

20 not required. 

The same can be said of the invention according to (5) below 
to which the notion of a virtual thrust transfer gate system is 
applied, and in the case of applying a control system of the present 
invention, which employs the notion of the virtual thrust transfer 

25 gate system, to a floating mobile object for floating in 
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three-dimensional space or the like, it is required to provide 
inclination angle measurement means capable of measuring an 
inclination of the floating mobile object as defined by the 
invention according to (6) below. 

[0025] Similarly, in the above control system for a floating mobile 
object according to the present invention, (3) the thrust transfer 
gate includes force/ torque measurement means capable of measuring 
a force/torque applied between the main body part and the effector 
part , the main body part is dynamically connected only to the thrust 
transfer gate, the main body part being adapted to receive 
substantially all forces acting thereon via the effector part and 
the thrust transfer gate, and the thrust command to the effector 
part is obtained by using: a target acceleration trajectory command 
obtained by converting a target position/velocity trajectory 
command from an operator; and an output from the force/torque 
measurement means from the thrust transfer gate. 

Note that in a typical example where the main body part 
is dynamically connected only to the thrust transfer gate and 
adapted to receive substantially all forces acting thereon via 
the effector part and the thrust transfer gate, the effector part 
is in the form of a shell enclosing the main body part. 

However, such a configuration is an extreme example, and 
the main body part is not necessarily sealed in an effector part 
having the form of a shell. In other words, it is only required 
to structure the invention defined in (3) such that the main body 
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part is not directly subjected to disturbances such as water 
pressure or intense wind pressure or other external forces, and 
if the main body part contained in a floating mobile object for 
sliding on water or ground, for example, is exposed to air, it 
5 is merely an ignorable problem that does not cause any particular 
inconvenience in application of the invention according to (3) . 

Thus, it is understood that the invention defined in (3) 
is not limited to the configuration as shown in FIG. 3. 
[0026] Further, in the control system for a floating mobile object 
10 according to the present invention as described in (2 ) or (3 ) above, 
(4) the main body part includes inclination angle measurement means 
capable of measuring an inclination of the main body part, and 
the thrust command to the effector part is obtained by additionally 
using an output from the inclination angle measurement means of 
15 the main body part. 

[0027] Further, the present invention is directed to (5) a control 
system for a floating mobile object, in which the floating mobile 
object is virtually in an aritlimetical manner divided into: amain 
body part that can be considered as a single rigid body constituting 
20 a part of the floating mobile object; and an effector part for 
generating a thrust for the floating mobile object, the floating 
mobile object includes acceleration measurement means capable of 
an acceleration of the floating mobile object, and an output from 
the acceleration measurement means is used to aritlimetically 
25 estimate a force/torque from the virtual thrust transfer gate to 
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obtain a thrust coiranand to the effector part. 

In addition, in the control system for a floating mobile 
object according to the present invent ion as described in (5) above, 
(6) the floating mobile object includes inclination angle 
measurement means capable of measuring an inclination of the 
floating mobile object, and the thrust command to the effector 
part is obtained by using outputs from the inclination angle 
measurement means and the acceleration measurement means to 
arithmetically estimate the force/ torque from the virtual thrust 
transfer gate. 

[0028] Note that as described below, in the above control system 
for a floating mobile object according to the present invention, 
control of the floating mobile object is carried out in two stages: 
a " thrust planning part " , which issues a command after calculating 
a thrust that should be applied from the thrust transfer gate to 
the main body part in order to realize a target trajectory, and 
a "thrust control part" , which directly drives the effector part, 
so as to realize a command thrust at the thrust transfer gate, 
by feeding back the current thrust value at the thrust transfer 
gate to the effector part. 

[0029] In the present invention, the above mechanism design and 
control technique are collectively referred to as the "thrust 
transfer gate system" . The thrust transfer gate system is mainly 
embodied in the following three forms: 

i) the main body part is exposed to the exterior, i.e.. 
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the main body part is directly affected by disturbances such as 
water pressure or intense wind pressure or other external forces, 
and a thrust transfer gate is further provided [see the 
below-described first instance of a first embodiment]; 

ii) a thrust transfer gate is provided, and in order not 
to subject the main body part directly to disturbances such as 
water pressure or intense wind pressure or other external forces, 
the main body part is dynamically connected only to the thrust 
transfer gate and adapted to receive substantially all forces 
acting thereon via the effector part and the thrust transfer gate 
(e.g., the main body part is covered and sealed in the effector 
part) [see the below-described second instance of the first 
embodiment] ; and 

iii) the floating mobile object is arithmetically divided 
into the main body part and the effector part without changing 
anyhardware component of the floating mobile object, and an inertia 
matrix for the main body part is previously prepared in a computer, 
and used together with an output of an acceleration sensor provided 
in the floating mobile object and, if necessary, an output from 
an inclination angle sensor provided in the floating mobile object , 
to obtain a thrust command to the effector part [see the 
below-described third instance of the first embodiment for a 
virtual thrust transfer gate] . 

[0030] Ini) andii) , achieved control performance is high, although 
it is required to change the hardware design of the floating mobile 
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object due to the thrust transfer gate system, and the force/ torque 
sensor is required at the thrust transfer gate. 

In iii), on the other hand, control performance is low 
compared to the methods in i) and ii) , but it is not required to 
change the hardware design of the floating mobile object, and by 
adding the acceleration sensor and, if necessary, the inclination 
angle sensor to an existing floating mobile object, it is made 
possible to utilize the thrust transfer gate system, thereby 
improving the control performance. 

Particularly, a known inert ial navigation device, which 
is currently mounted in aircrafts, etc. , can also be used as the 
acceleration sensor, inclination angle sensor or main body part 
position/velocity sensor used in the present invention, and by 
using this, it is made possible to realize in an increase in speed 
and precision compared to existing control, without making any 
hardware modification. 

[0031] In addition, the present invention is directed to (7) a 
floating mobile object acceleration sensing system for sensing 
an acceleration of a floating mobile object, in which the floating 
mobile object includes: a main body part that can be considered 
as a single rigid body constituting a part of the floating mobile 
object; an effector part for generating a thrust for the floating 
mobile object; and a thrust transfer gate for dynamically 
connecting the main body part and the effector part, the thrust 
transfer gate being adapted to be able to actually measure a thrust 
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from the effector part acting on the main body part, the thrust 
transfer gate includes force/torque measurement means capable of 
measuring a force/torque applied between the main body part and 
the effector part, the main body part is dynamically connected 
only to the thrust transfer gate, the main body part being adapted 
to receive substantially all forces acting thereon via the effector 
part and the thrust transfer gate, and an estimated acceleration 
value for the main body part is obtained by using an output from 
the force /torque measurement means from the thrust transfer gate. 

Also, in the floating mobile object acceleration sensing 
system according to the present invention as described in (7) above, 
(8) the main body part includes inclination angle measurement means 
capable of measuring an inclination of the main body part, and 
the estimated acceleration value for the main body part is obtained 
by additionally using an output from the inclination angle 
measurement means of the main body part. 

[0032] Accordingto the present invention, it is possible toprovide 
a system structure capable of allowing a floating mobile object 
to stand still in a predetermined position with high precision, 
or track a target trajectory with high precision, even under 
disturbances caused by waves and tidal current or wind or 
disturbances caused by reaction, etc., of a robot arm attached 
to the f loatingmobile object, therebymaking it possible to control 
the floating mobile object at higher speed with higher precision. 
[0033] Note that terms used for describing the present invention 
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are defined below. 

[0034] The term "floating mobile object" is meant to indicate 
various mobile objects for floating in water, air, outer space, 
and so on. Also, mobile objects for sliding on a plane or rails 
fall within the notion of the floating mobile object. 

Concrete examples falling within the notion of the "floating 
mobile object " as defined by the present invention are listed below 
by environmental type: 

(1) outer space : space robots, spaceships, 
artificial satellites, etc.; 

(2) air : aerial robots, 
aircrafts, helicopters, airships, etc.; 

(3) underwater : underwater robots, 
submersibles , etc . ; 

(4) overwater : watercrafts, air-cushion vehicles, 

etc . ; 

(5) ground : cars, trains, air-cushion 
vehicles, other mobile objects for sliding on ice or snow, such 
as sleds and snowmobiles, etc. 

As for ground vehicles such as cars, it is understood that 
they can be the "floating mobile object" when their wheels are 
sliding, for example. Accordingly, by applying the present 
invention to, for example, a car, it is possible to control the 
vehicle similarly regardless of whether its wheels are sliding 
or not . 



However, when the wheels are sliding, thrust transfer means 
composed of existing engine and tires cannot function. In view 
of this, it is understood from each of the following descriptions 
that when applying the present invention under the above 
circiamstance, it is preferred that the ground vehicle is provided 
with any other thrust transfer means (various types of thrusters 
or the like) . 

[0035] The term "main body part" is meant to indicate a physical 
structure that can be considered as a single rigid body constituting 
a part of the floating mobile object . In principle, the main body 
part does not include any mechanically moving part. Even when 
any mechanically moving part is included, no problem is caused 
if its influence is as small as it can be ignored, and when it 
cannot be ignored, still it is possible to cancel the influence 
if it is predictable. 

Note that in the invention that corresponds to the 
below-described third instance of the first embodiment according 
to the present invention to which the notion of the virtual thrust 
transfer gate system is applied, the rigid distinction between 
the main body part and the effector part is merely for convenience, 
users may be allowed to use their judgment to determine which part 
shouldbe defined as the main body par t . Accordingly, when actually 
carrying out the invention that corresponds to the below-described 
third instance of the first embodiment, the main body part may 
be defined as constituting the most part of the mass of the floating 
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mobile object , or on the contrary, themainbodypart may be defined 
as an object additionally provided in apart of the floating mobile 
object (e.g., a weight of about several kilograms). 

Also, in the inventions that correspond to the 
below-described first and second instances of the first embodiment 
to which the notion of the virtual thrust transfer gate system 
is not applied, whether or not the main body part and the effector 
part are connected by the below-described thrust transfer gate 
is the only judgment criterion for rigidly distinguishing between 
them, and there is no limitation on proportion of the main body 
part in the floating mobile object so long as such criterion is 
satisfied. 

Thus, in the present invention, the main body part is not 
limited to a physical structure composed of a single rigid body 
constituting themost part of the mass of the floatingmobile object . 
[0036] The term "effector part" is meant to indicate a physical 
structure for generating a thrust for the floating mobile object . 
All thrust generation means (effectors) for controlling the 
positiox:i, velocity, etc., of the floating mobile object are 
included. 

[0037] The term "dynamically connected" is meant to indicate a 
state where some Icindof interactionof force is talcing place between 
objects that are referred to thereby. In most cases, they are 
mechanicallyconnected, but cases where, evenwhen not mechanically 
connected, non-contact interaction of force is applied by, for 
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example, electromagnetic force, are also included. 

On the other hand, the term "mechanically divided" is meant 
to indicate a case where no interaction of force is applied between 
objects that are referred to thereby. 

[0038] The term "thrust transfer gate" is meant to indicate the 
only portion that dynamically connects the main body part and the 
effector part and includes a sensor capable of actually measuring 
all forces and torques applied between the main body part and the 
effector part, whereby it is possible to actually measure all 
thrusts from the effector part acting on the main body part. 

[0039] The term "virtual thrust transfer gate" is meant to indicate 
a thrust transfer gate, which is virtually provided in the floating 
mobile object and therefore is physically not present therein, 
and in an existing floating mobile object, which is arithmetically 
divided, for convenience's saJce, into a main body part and an 
effector part, it is adapted to be able to estimate, based on a 
measured acceleration value for the main body part, forces and 
torques applied between the main body part and the effector part, 
i.e. , all thrusts from the effector part acting on the main body 
part . 

Note that when only the term "thrust transfer gate" is used 
herein, the "virtual thrust transfer gate" is also included unless 
otherwise specified. 

[0040] The term "thrust estimated value at the virtual thrust 
transfer gate" is meant to indicate a virtual value obtained by 
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calculation based on a value measured at the virtual thrust transfer 
gate by an acceleration sensor (a main body part acceleration 
measured value) , rather than a value actually measured at a 
physically existing thrust transfer gate. 

[0041] The term "thrust planning part" is meant to indicate a part 
or function of a computer that issues a command to the thrust control 
part after calculating a desired thrust that should be applied 
at the thrust transfer gate from the effector part to the main 
body part, in order to realize a target trajectory. 

[0042] The term "thrust control part" is meant to indicate a part 
or function of the computer that outputs a thrust command to an 
effector, so as to realize a desired target thrust at the thrust 
transfer gate, by feeding bacJc a measured or estimated thrust value 
at the thrust transfer gate. 

[0043] The term "target thrust" is meant to indicate a thrust that 

should be applied at the thrust transfer gate from the effector 

part to the main body part, in order to realize a target trajectory. 

[0044] The "target acceleration" is meant to indicate an 

acceleration obtained by differential calculation based on a 

desired target trajectory or a target velocity. 

[0045] The term "thrust force transfer gate system (TTGS) " is meant 

to indicate a control system, which includes the thrust planning 

part and the thrust control part, and controls a floating mobile 

object provided with a thrust force transfer gate. 

[0046] The term "effector thrust force characteristic" is meant 
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to indicate the relationship between an effector thrust force 
command, which is inputted to an effector such as a thruster, and 
an effector thrust force, which is actually outputted. Note that 
the effector such as a thruster is generally generates a thrust 
f orcebymeans of a stream, the effector thrust force characteristic 
includes complicated dynamics affected by hydrodynamic 
resistance/ lift, in addition tomechanical dynamics of the effector 
itself . 

EFFECT OF THE INVENTION 

[0047] Conventionally, a thrust of a thruster is controlled by 
feeding backasensor information concerning the position/velocity 
of a floating mobile object, while the present invention directly 
feeds back a generated thrust to control a thrust of an effector, 
thereby realizing a high-speed response according to a principle. 
Thus, the above-mentioned sensing delay problem is alleviated. 
Particularly, when the thrust force transfer gate is provided as 
hardware (see, for example, the first and second instances of the 
first embodimenc) , che influence of sensing delay is not increased 
even if the mass of the floating mobile object is large or the 
thrust of the effector is low. 

[0048] In a floating mobile object control system of the present 
invention, the thrust of the effector is controlled by a thrust 
control part feeding back a thrust at the thrust transfer gate 
as described below. This realizes the above-mentioned high-speed 
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response. At the same time, by applying to the thrust control 
part an existing control technique (e.g. , sliding mode control) , 
which offers robustness to nonlinearities , it is possible to 
suppress a nonlinearity of an effector dynamic characteristic, 
5 thereby preventing adverse influence on control performance. 
Conventionally, linearization of the effector dynamic 
characteristic is performed by feedforward compensation based on 
a dynamic characteristic model of the effector, however, it is 
significantly time-consuming and imposes a heavy computation load . 
10 The floating mobile object control system of the present invention 
realizes the linearization by thrust feedback, which does not 
necessarily use the dynamic characteristic model, and therefore 
it is possible to construct a control system with less computation 
load. 

15 [0049] In addition, in the conventional art, an unignorable delay 
in time occurs during a period of time since sensing is carried 
out as described above until a thrust command is actually fed to 
the thruster, and as a result, in order to keep the floating mobile 
object on a predetermined trajectory, it is inevitable, in most 

20 cases, to drive the floating mobile object more than necessary, 
for example, because it is required to repeatedly correct an 
overshoot and an undershoot a considerable number of times. The 
task of suitably performing gain tuning of the control system is 
extremely significant and requires experience in addition to labor 

25 and time. 
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On the other hand, according to the present invention, it 
ispossible to feeda thrust that is actually required by the thruster 
in a timely manner, and therefore it is possible to avoid driving 
the thruster and the floating mobile object more than necessary. 
5 In addition, the floating mobile object is not caused to consume 
thruster drive energy more than necessary, whereby it is possible 
to achieve an effect of reducing energy consumption of the floating 
mobile object, 

[0050] Also, in the floating mobile object control system of the 

10 present invention, influence of unknown disturbances can be 
immediately sensed as thrust error at the thrust transfer gate, 
and compensated for by local feedback control of the thrust control 
part, and therefore levels above the thrust planning part are not 
required to take the disturbances into consideration. 

15 Specifically, by only requiring the thrust planning part to issue 
a command such as "stand still" or "go straight", it is possible 
to automatically compensate for the influence of the disturbances 
in the thrust control part, thereby realizing a desired operation 
that is previously planned. Also, it is possible to realize a 

20 high-speed response compared to a conventional case where the 
influence of the disturbances is sensedbypos i t ion/velocity error . 
[0051] On the other hand, according to a floating mobile object 
acceleration sensing system of the present invention, an estimated 
acceleration value for the main body part can be obtained by using 

25 a previously identified inertia matrix of the main body part, an 
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output from the force/ torque sensor from the thrust transfer gate, 
and, if necessary, an output from the inclination angle sensor 
of the main body part, and therefore it is possible to sense an 
acceleration of the floating mobile object with high precision 
to such an extent that a minor change in acceleration can be sensed, 
regardless of size and weight of the floating mobile object or 
without depending on the capability of the acceleration sensor, 
and also possible to, when guiding the floating mobile object by 
inertial navigation, accurately guide the floating mobile object 
onto a desired trajectory. 

Hereinafter, the present invention will be described in 
more detail. 

BRIEF DESCRIPTION OF THE DRAWINGS 

[0052] [FIG. IJ FIG. 1 is a schematic diagram illustrating an 
exemplary configuration of the floating mobile object to which 
a control system of the present invention is applied, 

[FIG. 2] FIG. 2 is a block diagram illustrating an exemplary 
configuration of the control system of the present invention. 

[FIG. 3] FIG. 3 is a schematic diagram illustrating an 
another exemplary configuration of the floating mobile object to 
which the control system of the present invention is applied. 

[FIG. 4] FIG. 4 is a block diagram illustrating an another 
exemplary configuration of the control system of the present 
invention. 
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[FIG. 5] FIG. 5 is a schematic diagram illustrating an 
another exemplary configuration of the floating mobile object to 
which the control system of the present invention is applied. 

[FIG. 6] FIG. 6 is a block diagram illustrating an another 
exemplary configuration of the control system of the present 
invention. 

[FIG. 7] FIG. 7 is a block diagram illustrating a second 
embodiment of the control system of the present invention. 

[FIG. 8] FIG, 8 is a block diagram illustrating an example 
of the control system of the present invention. 

[FIG. 9] FIG. 9 is a block diagram illustrating an another 
example of the control system of the present invention. 

[FIG. 10] FIG. 10 is ablockdiagram illustrating an another 
example of the control system of the present invention. 

[FIG. 11] FIG. 11 is a diagram schematically illustrating 
a prior known floating mobile object. 

[FIG. 12] FIG. 12 is a block diagram schematically 
illustrating a prior known floating mobile object control system. 

[FIG . 15 ] FIG . 13 is an another block diagram schematically 
illustrating a prior known floating mobile object control system. 

[FIG. 14] FIG. 14 is an another block diagram schematically 
illustrating a prior known floating mobile object control system. 

DESCRIPTION OF THE REFERENCE CHARACTERS 
[0053] A robot arm part 



B main body part 

Bl statics of main body part 

B2 inertia of main body part 

B2 ' inertia of main body part 

B3 inertia of main body part 

B4 integration 

C cable 

E effector' part 

El effector thrust characteristic 

E2 statics of effector part 

G thrust transfer gate 

G' virtual thrust transfer gate 

H operator 

P computer 

Pi differential calculus 

P2 thrust planning part 

P3 thrust control part 

P4 position control /velocity control 

S pressure hull 

T thruster 

1 control system 

2 acceleration sensor 

3 inclination angle sensor 

4 position/velocity sensor 

5 cavity 
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10 floating mobile object 

11 subtraction circuit 

12 thruster control circuit 

13 sensor 

5 14 effector thrust characteristic 

15 floating mobile object dynamics 

16 reaction force/torque of robot arm 

17 adding circuit 

18 thruster/robot arm control circuit 
10 19 joint actuator 

20 floating mobile object main body /robot arm complex 
system dynamics 

21 subtraction circuit 

15 EMBODIMENTS FOR CARRYING OUT THE INVENTION 
[0054] [First Embodiment] 

Hereinafter, an embodiment of the present invention and 
the action thereof will be described with reference to FIGS 1 
through 7 in relation to an underwater robot, which typifies a 

20 target for which the present invention is carried out. The 
underwater robot is nothing less than a floating mobile object 
for floating in three-dimensional space, and in the case of applying 
a control system of the present invention to the underwater robot, 
it is necessary to compensate for influence of a gravitational 

25 acceleration, therefore it is required toprovide inclinationangle 
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measurement means capable of measuring an inclination of a main 
body part as defined by the invention according to (4) above. The 
same can be said of the invention according to (5) above to which 
the notion of a virtual thrust transfer gate system is applied, 
5 and in the case of applying a control system of the present invention , 
which employs the not ion of the virtual thrust transfer gate system, 
to a floating mobile object for floating in three-dimensional space 
or the like, it is required to provide inclination angle measurement 
means capable of measuring an inclination of the floating mobile 

10 object as defined by the invention according to (6) above. 

The inventions according to (2) and (3) above further 
including inclination angle measurement means, which is the 
invention according to (4) above, correspond to first and second 
cases of a first embodiment , respectively, and they also correspond 

15 to the below-described examples 1 and 2, respectively. The 
invention according to (6) above corresponds to a third case of 
the first embodiment and the below-described example 3. 

It is fully understood from the above definition section 
and the below-described examples that the application target is 

20 not limited to the underwater robot. In addition, the underwater 
robot is not limited to a typical example used for deep sea 
exploration as illustrated in the drawings, and relatively small 
robots, which can be used for lifesaving or oil field exploration, 
fall within the category of the underwater robot. 

25 Here, FIG. 1 is a schematic diagram illustrating an exemplary 



configuration of the floating mobile object to which a control 
system of the present invention is applied, and FIG. 2 is a block 
diagram illustrating an exemplary configuration of the control 
system of the present invention. FIGS. 3 and 5 are schematic 
diagrams each illustrating an another exemplary configuration of 
the floatingmobile object to which the control systemof the present 
invention is applied, and FIGS. 4 and 6 are block diagrams each 
illustrating an another exemplary configuration of the control 
system of the present invention. FIG, 7 is a block diagram 
illustrating a second embodiment of the control system of the 
present invention . 

In FIGS, 1 through 7 above, the same elements as those 
illustrated in FIGS . 11 through 14 illustrated above are described 
with the same reference characters. 

In FIGS. 2,4,6 and 7, a thin solid line extending between 
blocks represents a signal. In contrast, a double line extending 
between blocks within a floating mobile object 10 represents a 
phenomenon (dynamic transformationactionor the like) takingplace 
as a physical accion in the floating mobile object 10. 
[0055] Incidentally, in the following detailed description of the 
present invention, a reference coordinate system fixed in the 
absolute location of an inertial system is denoted by Zr, and a 
main body coordinate system having the origin at the gravitational 
center position of the floating mobile object and fixed on the 
main body part of the floating mobile object is denoted by Ib- 



Unless otherwise specified, vectors and matrices are indicated 
by expressions as they are viewed from the main body coordinate 
system Zb- Further, the floating mobile object is present in 
m-dimensional space {m< 6) , and an effector of the floating mobile 
object is capable, of generating an n-degree-of-f reedom thrust (m 
< n) . 

[0056] First Instance 

The configuration of a first instance of a thrust transfer 
gate system according to the present embodiment is as shown in 
FIGS. 1 and 2 . FIG. 1 is a schematic diagram of a floating mobile 
object, and FIG, 2 is a block diagram of a control system. 

In the present instance, a floating mobile object 10 is 
dynamically divided into a main body part B and an effector part 
E, and a thrust transfer gate G is provided as hardware between 
the main body part B and the effector part E. In addition, the 
main body part B is provided with an acceleration sensor 2 and 
an inclination angle sensor 3, and the thrust transfer gate G is 
provided with a force/torque sensor. In the present instance, 
the main body part B and the thrust transfer gate G are configured 
to be exposed to exterior similar to the effector part E. 

Note that the below-described inertiamatrix Af^ of the main 
body part B and J^^ and J^^ representing geometric constructions 
of the floating mobile object 10 are given as previously measured 
known matrices . 

[0057] Hereinbelow, the configuration of the present instance is 
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described in detail. 

Referring to FIG. 2 for the interiors of the main body part 
B and the effector part E, an equation of motion representing 
physical dynamics of the floating mobile object can generally be 
written as the following equation. 
[0058] [Expression 1] 

JcB^^ci^) = MBMt)'gs{t))^M^a,{t)'-f,{t) (1) 

Wc(0 = ^£/(0«e(0 + /£(0 (2) 
Equation (1) represents the main body part, and Equation (2) 
represents the effector part . Each variable is defined as follows : 

0^(1) gR"" : an acceleration of the main body part 

(gravitational center) of the floating mobile object; 

ggit) sR"" : a gravitational acceleration applied to the 

main body part (gravitational center) of the floating mobile 
object; 

m^COg/?'" : a thrust from the effector part acting on 

the main body part at the thrust transfer gate; 

y^jjG : a Jacobian matrix for transformation from 
the velocity of the main body part to the velocity of the thrust 
transfer gate; 

eR""""" : an inertia matrix of the main body part of 

the floating mobile object; 

e R'^'^ : an additional inertia matrix for the main 

bodypart of the floating mobile object due to influence of a stream; 

fB{t)sR'" : a thrust (an external force such as tidal 
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current , waves or wind) acting on the main body part of the floating 
mobile object, which cannot be expressed by other terms in Equation 
(1) ; 

WfCOe^" : a thrust generated by each effector; 

5 -^EcCO s^"'*'" : a Jacobian matrix for transformation from 

the velocity of the thrust transfer gate to the velocity of each 
effector; 

: a thrust (e.g., an inertia force of an 
effector part, or external force such as tidal current, waves or 
10 wind) from the effector part of the floating mobile object acting 
on the thrust transfer gate, which cannot be expressed by other 
terms in Equation (2) , and is indicated by an expression as it 
is viewed from the thrust transfer gate. 

[0059] Note that the thrust transfer gate system 1 of the present 
15 instance is structured such that the thrust transfer gate G is 
fixed on the single rigid body structure of the main body part 
B. Thus, the Jacobian matrix J^^ and the inertia matrix are 
constant matrices. 

[0060] Described next is a thrust planning part of the present 
20 instance. 

A target thrust t<^^(0 at the thrust transfer gate G required 
for realizing a target acceleration aQj{{) of the main body part 
B as indicated by the bloclc of a thrust planning part P2 within 
a computer P in FIG. 2 is defined by the following equation. 
25 [Expression 2] 



"c^ (0 = (0 + Jcb'^^b {(^Bd (0 - a, (/)) ( 3 ) 

[0061] Note that a^^(0 and ^^(0 [an m-dimensional vector] are 
respectively a target value and an estimated value for a main body 
acceleration a^it) , and w^^(0 [an m-dimensional vector] is a target 
value for a gate tlirust u^it) , and u^^it) [an m-dimensional vector] 
is a value of u^(0 measured by a force/torque sensor provided at 
the thrust transfer gate. 

[0062] Here, by controlling m^(0 with a thrust control part denoted 
by reference character P3 within the computer P in FIG. 2, such 
that Uq^(J) is realized by thrust feedbaclc, it is possible to 
construct a feedbaclc control system for controlling a thrust by 
thrust feedback with less delay. 

A control technique offering superior robustness to 
nonlinear systems should be applied to the thrust control part 
P3 in order to suppress an unknown nonlinearity of an effector 
thrust characteristic denoted by reference character El within 
an effector part E in FIG. 2 and a dynamic characteristic of the 
effector part such as disturbances or dynamic force (/) against 
the effector part . For example, by applying existing sliding mode 
control, the thrust control part P3 can be constructed as below. 
[0063] In the thrust control part P3 of the present instance, a 
thrust command u^^it) to the effector required for realizing the 
target thrust u^^it) , is defined as in the following equation. 
[0064] [Expression 3] 
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k./max(k(0],<0) 

k.(Ol= 0 (k(0]/=0) (5) 

[«£..mJkW].> 0) 



Each variable is defined as follows: 

SqQ) e A" : a switch function for sliding mode control ; 

5 Tq^R : a time constant of a first-order lag system 

constrained by switch function; 

u^j{t)eR" : a command thrust to each effector; 

"E</»max^^ • a maximum commandable thrust to an i-th 

component of the effector thrust; 
10 ^Edimin^^ ' ^ minimum commandable thrust to the i-th 

component of the effector thrust; 

: an i-th component of a vector * 

: transposition of a pseudo inverse matrix 

of a matrix *. 

15 [0065] In the present instance, values required for feedback to 
Che thrust planning part and the thrust control part are u^.U) 
and dg{t) , As the gate thrust measured value m^j(0 / a value measured 
by the force/torque sensor provided at the thrust transfer gate 
is used as it is. 

20 [0066] Described below is a method for estimating the acceleration 
Ogit) of the main body part. To examine a sensor characteristic 
and a thrust control characteristic, the following discussion is 
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given mainly in relation to a frequency domain. 
[0067] First, a measured acceleration value ^^,(0 tcin 
m-dimensional vector] obtained by tlie acceleration sensor of the 
main body part denoted by reference cliaracter 2 within the main 
body part B in FIG. 2 is given as below. Themeasured value includes 
a gravitational acceleration, and therefore cannot be used as an 
estimated value for the main body acceleration ag(t) without any 
change . 

[0068] [Expression 4] 

^Bs(s) = G,,^is)ia,is)^g,(s)) (6) 

[0069] Note that *(s) is Laplace transform of the vector or matrix 
* (*(^) = ^*(0]) / and G^B^(s) is a transfer function matrix with m 
rows and m coltimns representing an acceleration sensor 
characteristic of the main body part. 

Similarly, a measured value gg^iO [an m-dimensional 
vector] for a gravitational acceleration obtained by the 
inclination angle sensor of the main body part is given as below. 

[0070] [Expression 5] 

[0071] Note that G^g^is) is a transfer function matrix with m rows 
and m columns representing an inclination angle sensor 
characteristic of the main body part. 

Similarly, a gate thrust measured value [an 
m-dimensional vector] obtained by the thrust transfer gate is given 
as below. 
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[0072] [Expression 6] 

^Gs(s) = G,cs(s)uais) (8) 
[0073] Note that is a transfer function matrix with m rows 

and m columns representing a force/torque sensor characteristic 
5 of the thrust transfer gate. 

[0074] Described next is estimation of the main body acceleration 
ag(t) in the present instance. 

An estimated value a^it) for the main body acceleration 
a^it) is defined as below. 
10 [0075] [Expression 7] 

^.(0 = «z^.(0 + gi,.(0 (9) 
[0076] Here, consider the validity of the above estimation of the 
main body acceleration. By applying the Laplace transform to 
equation (9) and substituting equations (6) and (7), 

15 a,(s) = G^,^{s){a,(s)-g,{s))^G^,^(s)^^^^ (9) ' 

is given, and if the condition "all types of sensor characteristics 
are equal at all degrees of freedom and there is no interference 
between sensors and between the degrees of freedom (a sensor 
characteristic consistent condition) " is satisfied, the 

20 acceleration sensor characteristic of the main body part, the 
inclination angle sensor characteristic of the main body part and 
the force/torque sensor characteristic of the thrust transfer gate 
may be defined as follows: 

GaBs(^) = = G^asi^) = G,is)I ( 10 ) , 

25 where G^(s) is a transfer function representing an equal sensor 
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characteristic, and I is a unit matrix. That is, 
ciBs(^) = G,is)(a,(s)-g,{s) (11), 

gBs(^) = G^(^)gB(^) (12), and 

^csi^) = G,is)u^is) (13) . 

In this case, equation (9) ' is given as below. 
[0077] [Expression 8] 

^B(^) = G,(s)a,{s) (14) 
[0078] Thus, the estimation of the main body part acceleration 
ag{t) can be performed by equation (8). A gravitational 
acceleration gg^(t) does not explicitly appear in equation (14), 
and therefore it is possible to consider that the main body part 
acceleration ag(t) is directly measured so long as the above sensor 
characteristic consistent condition is satisfied. 

Note that the above sensor characteristic consistent 
condition is realized by, for example, selecting a sufficiently 
high-speed response and high-precision sensor compared to the 
dynamics of the floating mobile object. 
[0079] Second Instance 

Described next is a second instance of the thrust transfer 
gate system according to the present embodiment. 

The second instance of the thrust transfer gate system is 
configured as shown in FIGS . 3 and 4 . FIG . 3 is a schematic diagram 
of a floating mobile object, and FIG. 4 is a block diagram of a 
control system. 

In the present instance also, a floating mobile object 10 
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is dynamically divided into a main body part B and an effector 
part E, and a thrust transfer gate G is provided as hardware between 
the main body part B and the effector part E. In addition, the 
main body part B is provided with an inclination angle sensor 3, 
and the thrust transfer gate G is provided with a force/torque 
sensor. 

The present instance significantly differs from the first 
instance in that the effector part E is a shell for covering the 
main body part B (a pressure hull S shown in FIG. 3) , and therefore 
the main body part B and the thrust transfer gate G are completely 
isolated from the exterior by the shell S. This structure 
eliminates the necessity of the acceleration sensor 2 of the main 
body part B, which is required in the first instance. 

As in the first instance, an inertia matrix of the main 
body part and J^^ and J representing geometric constructions of 
the floating mobile object are given as previously measured known 
matrices . 

[0080] Hereinbelow, the configuration of the present instance is 
described in detail. 

Referring to FIG. 4 for the interiors of the main body part 
B and the effector part E, an equation of motion representing 
physical dynamics of the floating mobile object can generally be 
written as the following equation. 
[0081] [Expression 9] 

JcB^tic (0 = M^ (a, (/) - (/)) (15) 
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«g(0 = ^£g'(0«£(0 + /£(0 (16) 

[0082] Equation (15) represents the main body part , and equation 
(16) represents the effector part . Here, Wc2(0 [anm-dimensional 
vector] is defined anew as in the following equation. 
[0083] [Expression 10] 

[0084] Described next is a thrust planning part in the present 
instance. 

A target thrust u^^d^O at the thrust transfer gate G required 
for realizing a target acceleration a^^(0 of the main body part 
B as indicated by the bloclc of the thrust planning part P2 within 
a computer P in FIG. 4 is defined by the following equation. 
[0085] [Expression 11] 

^G2d (0 = ^CB~^^ B^Bd (0 (18) 

[0086] By controlling u^^it) with a thrust control part denoted by 
reference character P3 within the computer P in FIG. 4, such that 

w^2(/(0 is realized by thrust feedbacJc, it is possible to construct 
a feedbacJc control system for controlling a thrust by thrust 
feedbaci^ with less delay. 

Here, for example, by applying slidingmode control to thrust 
control in the present instance, it is possible to construct the 
thrust control part P3 as below. 

[0087] In the thrust control part P3 of the present instance, a 
thrust command u^^{t) to the effector required for realizing a 
target thrust u^idiO is defined as in the following equation. 
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[0088] [Expression 12] 



^c(0 = ^Ec''(0|("a2(0-«c2.(0) + 7i^^ (19) 



«£..„^(k(0]/<0) 

0 (k(0]/=0) (20) 

«£rf/nun(k(0],>0) 



[0089] Note tliat Uc2iO [an m-dimensional vector] is an estimated 
5 value for m^jW • 

[0090] Incidentally, in the present instance, no feedbaclc to the 
thrust planning part P2 is required, and only the estimated value 
11^2(0 ^or M^2(0 equation (17) is required for feedbacJc to the 
thrust control part P3 . Here, a method for estimating 1/^2(0 is 
10 described. 

[0091] Hereinbelow, estimation of the gate thrust u^^it) in the 
present instance is described. 

The estimated value m^2(0 for the gate thrust u^^iO is 
defined as in the following equation. 
15 [0092] [Expression 13] 

«C2 (0 = ^Gs (0 + Jgb''^ bSbs (0 (21) 
[0093] Here, consider the validity of the estimation of the gate 
thrust. By applying the Laplace transform to equation (21), 

^G2(^) = ^csi^)'^^CB~'^Msg8s(s) (21) • 

20 is given, and if the aforementioned sensor characteristic 
consistent condition is also satisfied here, equations (12) and 
(13) are established similarly. By substituting these and 
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considering equation (17), the following is given. 
[0094] [Expression 14] 

tic2is) = G^(s)u^,{s) (22) 
[0095] Thus, the gate thrust Wc2(0 can be estimated by equation 
(21) . A gravitational acceleration gg^it) does not explicitly 
appear in equation (22) , and therefore it is possible to consider 
that the gate thrust u^^iO is directly measured so long as the 

sensor characteristic consistent condition is satisfied. 
[0096] Third Instance 

Described next is a third instance of the thrust transfer 

gate system according to the present embodiment. 

The third instance of the thrust transfer gate system is 

configured as shown in FIGS . 5 and 6 . FIG. 5 is a schematic diagram 

of a floating mobile object, and FIG. 6 is a block diagram of a 

control system. 

In the third instance, the floating mobile object 10 is 

not dynamically divided into a main body part B and an effector 

part E, and no thrust transfer gate G is provided as hardware. 

However, the floating mobile object 10 of the present instance 

is provided with an acceleration sensor 2 and an inclination angle 

sensor 3 . 

In the present instance, a thrust transfer gate system is 
constructed as if a thrust transfer gate is present by suitably 
determining an inertia matrix Mg of a main body part B and 
calculating, based on , an estimated thrust value at a virtual 



thrust transfer gate G* shown within a computer P in FIG. 6 from 
a value measured by the sensor. As in the above first and second 
instances, J^^ and J^^ representing geometric constructions of 
the floating mobile object 10 are given as previously measured 
known matrices. 

Note that examples of a method for obtaining the inertia 
matrix ot the main body part B include a method in which the 
computer previously calculates the inertia matrix of the main 
bodypartB based on drawings, specifications, etc. , of the floating 
mobile object 10 as originally designed and stores it in an 
unillustrated memory or the like. 

[0097] Hereinbelow, the configuration of the present instance is 
described in detail. 

Referring to FIG. 6 for the interiors of the main body part 
B and the effector part E, an equation of motion representing 
physical dynamics of the floating mobile object can generally be 
written as the following equation. 
[0098] [Expression 15] 

^/«C7v(0=^^B«Z,(0 (23) 

"gv(0 = ^^a'CO^f (0 + AW + JcB'^B&sit) ( 24 ) 

[0099] Equation (23) represents the main body part , and equation 
(24) represents the effector part . This is approximately the same 
as in the second instance, but it should be noted that there is 
no physical thrust transfer gate in the third instance, and 
therefore a thrust u^^ [an m-dimensional vector] at the thrust 
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transfer gate is a virtual value. 

[0100] Described next is a thrust planning part of the present 
instance. 

A target thrust w^vrfCO at a virtual thrust transfer gate 
5 G' required for realizing a target acceleration ag^{t) of the main 
body part B as indicated by the block of a thrust planning part 
P2 within a computer P in FIG, 6 is defined as in the following 
equation. 

[0101] [Expression 15] 



[0102] By controlling w^vW with a thrust control part denoted by 
reference character P3 within the computer P in FIG. 6, such that 
^cvdiO is realized by thrust feedbaclc, it is possible to construct 
a feedbaclc control system for controlling a thrust by thrust 
15 feedback with less delay. 

Here, for example, byapplyingslidingmode control to thrust 
control in the present instance, it is possible to construct the 
thrust control part P3 as below. 

[0103] In the thrust control part P3 of the present instance, a 
20 thrust command u^^Q) to the effector required for realizing the 
target thrust UQ^{t) is defined as in the following equation. 
[0104] [Expression 17] 



10 



(25) 
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"E<//max(kW]/<0) 

0 (k(0]/=0) (27) 



[0105] Note that ^^^,(0 m-dimensional vector] is an estimated 
value for u^^it) . 

[0106] Incidentally, in the present instance, no feedbaclc to the 
5 thrust planning part P2 is required as in the second instance, 
and only the estimated value w^vCO for u^^it) is required for 
feedbacJc to the thrust control part P3 . Here, a method for 
estimating u^XO is described. 

[0107] Hereinbelow, estimation of the gate thrust w^^CO in the 
10 present instance is described. 

The estimated value u^^t) for the gate thrust u^^t) is 
defined as in the following equation. 
[0108] [Expression 18] 

«cv(0 = -/c/^M, + g,,(0) (28) 
15 [0109] Here, consider the validity of the estimation of the gate 
thrust. By applying the Laplace transform to equation (28), 

WcvU) = -'c5"^fi(«s.U) + g55U)) (28) ' 
is given, and if the aforementioned sensor characteristic 
consistent condition is also satisfied here, equations (11) and 
20 (12) are established similarly. By substituting these and 
considering equation (23), the following is given. 
[0110] [Expression 19] 

iicM = G,{s)u^,is) (29) 
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[0111] Thus, the gate thrust u^^it) can be estimated by equation 
(28) . A gravitational acceleration g^(0 does not explicitly 
appear in equation (29) , and therefore it is possible to consider 
that the gate thrust u^^it) is directly measured so long as the above 
sensor characteristic consistent condition is satisfied. 
[0112] [Second Embodiment] 

Described next is an embodiment of a floating mobile object 
acceleration sensing system of the present invention. As in the 
first embodiment, the present embodiment is described in relation 
to an underwater robot as an application target. Note that the 
underwater robot is nothing less than a floating mobile object 
for floating in three-dimensional space, and in the case of applying 
an acceleration sensing system of the present invention to the 
underwater robot, it is necessary to compensate for influence of 
a gravitational acceleration as in the first embodiment , therefore 
it is required to provide inclination angle measurement means 
capable of measuring an inclination of a main body part as defined 
by the invention according to (8) above. 

In the present embodiment, a thrust transfer gate is used 
for estimating an acceleration of the main body part. The 
acceleration sensing system according to the present embodiment 
is configured as shown in FIGS. 3 and 7. FIG. 3 is a schematic 
diagram of a floating mobile object, and FIG. 7 is a block diagram 
of the acceleration sensing system. 

[0113] Ineach instance of the first embodiment , the thrust transfer 



49 



gate system is constructed as the control system 1 for the floating 
mobile object 10, whereas in the second embodiment, a thrust 
transfer gate G is used as a high-precision acceleration sensing 
system for the floating mobile object 10. 
5 [0114] Hardware configuration of the present embodiment is exactly 
the same as that of the second instance (FIG. 3) according to the 
above first embodiment. Specifically, the floating mobile object 
10 is dynamically divided into a main body part B and an effector 
part E, the main body part B is provided with an inclination angle 

10 sensor 3, and the thrust transfer gate G is provided with a 
force/ torque sensor. 

However, the present embodiment aims to estimate an 
acceleration ag(t) of themainbody part with high precision, rather 
than control it , and therefore the present instance does not include 

15 the thrust planning part and the thrust control part. 

In addition, an inertia matrix Mg of the main body part 
B and J^g representing a geometric construction of the floating 
mobile object are given as previously measured known constant 
matrices. By suitably designing the inertia matrix Mq of the main 

20 body part with consideration of the measuring range of the 
force/ torque sensor at the thrust transfer gate and an acceleration 
range that should be sensed in the present embodiment, it is made 
possible to measure an acceleration with high precision. 
[0115] Hereinbelow, the configuration of the present embodiment 

25 is described in detail. 
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The acceleration a^it) of the main body part in the hardware 
configuration of the present embodiment as shown in FIGS. 3 and 
7 is represented by transforming equation (15) as below. 
[0116] [Expression 20] 

5 a, (t) = Mi'jGs\(0 -^gaiO (30) 

[0117] Described next is estimation of the acceleration Qgit) of 
the main body part in the present embodiment. 

An estimated value a^Q) for the acceleration Cgit) of the 
main body part is defined as in the following equation. 
10 [0118] [Expression 21] 

(0 = ^b'^Jcb'^gs (0 + gBs (0 (31) 
[0119] Here, consider the validity of the estimation of the main 
body acceleration. By applying the Laplace transform to equation 
(31) , the following is given. 
15 [0120] [Expression 22] 

a sis) = Mq'^JcbUc,{s) + gs,{s) ( 32 ) 

[0121] If the aforementioned sensor characteristic consistent 
condition is also satisfied here, equations (12) and (13) are 
established as in the first embodiment. By substituting these 
20 and considering equation (30) , the following is given. 
[0122] [Expression 23] 

a,{s)^G^{s)o^{s) (33) 
[0123] Thus, it is appreciated that the main body part acceleration 
can be estimated by equation (31) . 
25 [0124] Example 1 
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As described in the above first embodiment section, the 
floating mobile object control system of the present invention 
performs thrust control, and therefore, to be strict, an 
acceleration trajectory, rather than a position/velocity 
trajectory, is realized. Here, it is not possible to avoid 
influence of drift caused by integration error in acceleration, 
and therefore the motion on which the present invention solely 
exhibits a remarkable effect is that in a relatively high frequency 
range. As for low- frequency slow motion, it is necessary to 
additionally use a conventional control technique for feeding back 
a position/velocity, but in such a case also, it is possible to 
improve precision compared to the case of using the conventional 
technique alone. 

Hereinbelow, an example of the present invention is 
described in relation to a control system employing conventional 
position/velocity feedback control in addition to each instance 
of the floating mobile object control system of the present 
invention as described in the above first embodiment. 
L0125J Here, FIG. 8 is a block diagram illustrating an example 
of the control system of the present invention corresponding to 
the first instance of the first embodiment , FIG . 9 is a block diagram 
illustrating an another example of the control system of the present 
invention corresponding to the second instance of the first 
embodiment, and FIG. 10 is a block diagram illustrating an another 
example of the control system of the present invention 
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corresponding to the third instance of the first embodiment. In 
FIGS. 8 through 10, the same elements as those shown in FIGS. 1 
through 7 and 11 through 14 above are described with the same 
reference characters. 
5 In addition, thin solid lines extending between blocks in 

FIGS 8 through 10 represent signals as in FIGS. 2, 4, 6 and 7. 
On the other hand, double lines extending between blocks in a 
floating mobile object 10 represent phenomena (dynamic 
trans formation action or the like) takingplace as aphysical action 

10 in the floating mobile object 10. 

[0126] Example 1 corresponds to the above first instance of the 
first embodiment , FIG . 8 illustrates a block diagram of a control 
system 1 according to the present example. A schematic 
configuration of the floating mobile object 10 of the present 

15 example is as shown in FIG. 1. 

As is apparent from the block diagram in FIG. 8, in the 
present example, the configuration shown in FIG. 2 and described 
in the first instance of the first embodiment is applied to a prior 
known position/velocity feedback control system. Accordingly, 

20 the configuration of a thrust transfer gate system portion of the 
present invention, the operation of the control system, etc. , are 
described with reference to FIGS. 1 and 2 above. 
[0127] The control system 1; i.e., the thrust transfer gate system 
according to the present example, is mainly characterized in that 

25 the mechanism of the floating mobile object denoted by reference 
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character 10 is dynamically divided into a main body part B, which 
receives a thrust , andanef fector part E, which generates a thrust, 
and a thrust transfer gate G is added to a connection therebetween. 
Here, the thrust transfer gate G is an only portion that connects 
the main body part B with the effector part E and includes a sensor 
(a force/torque sensor) capable of actually measuring all forces 
and torques applied between the main body part B and the effector 
part E. Note that as the force/torque sensor, a commercially 
available general-purpose product (e.g. , a multi-channel product 
capable of outputting multi-dimensional force or torque 
components) can be used. 

As such, in the present example, by connecting the main 
body part B and the effector part E with the thrust transfer gate 
G, all thrusts from the effector part E acting on the main body 
part B can be measured. In the floating mobile object 10 of the 
present example towhich the thrust transfer gate systemis applied, 
the main body part B is provided with an acceleration sensor 2 
and an inclination angle sensor 3 as shown in FIG. 1. 
[0128] The thus -configured control system 1 according to the 
present example performs thrust feedback control by the thrust 
transfer gate system of the present invention simultaneously as 
prior known position/velocity feedback control. 
[0129] According to the present example, while additionally using 
a conventional control technique for feeding back a 
posit ion/ velocity to avoid influence of drift caused by integration 
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error in acceleration, a control system, i.e. , a thrust transfer 
gate system of the present invention, is used to allow a floating 
mobile object to stand still in a predetermined position with high 
precision, or track a target trajectory with high precision, even 
under disturbances caused by waves and tidal current or wind or 
disturbances caused by reaction, etc., of a robot arm attached 
to the floating mobile object, thereby making it possible to control 
the floating mobile object at higher speed with higher precision. 
[0130] Example 2 

The present example corresponds to the above second instance 
of the first embodiment. FIG. 9 illustrates a block diagram of 
a control system 1 according to the present example, A schematic 
configuration of a floating mobile object 10 of the present example 
is as shown in FIG. 3 above. 

As in example 1, the control system 1 according to the present 
example is mainly characterized in that the mechanism of the 
underwater robot 10 is dynamically divided into a main body part 
B, which receives a thrust , and an effector part E, which generates 
a Lhrusc, and a thrust transfer gate G is added to a connection 
therebetween. 

Note that the present example differs from the 
above-described example 1 in that the main body part B is contained 
in the effector part E, and external disturbances, reaction of 
a robot arm A, etc., all affect the effector part E, which is a 
pressure hull S. Here, the main body part B is supported only 
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by the thrust transfer gate G in the effector part E, Therefore, 
as in the first example, the thrust transfer gate G is an only 
portion that connects the main body part B with the effector part 
E. 

Accordingly, in the present example also, by connecting 
the main body part B and the effector part E with the thrust transfer 
gate G, all thrusts from the effector part E acting on the main 
body part B can be actually measured. 

[0131] According to the second example also, while additionally 
using a conventional control technique for feeding back a 
position/velocity to avoid influence of drift caused by integration 
error in acceleration, a control system, i.e. , a thrust transfer 
gate system of the present invention, is used to allow a floating 
mobile object to stand still in a predetermined position with high 
precision, or track a target trajectory with high precision, even 
under disturbances caused by waves and tidal current or wind or 
disturbances caused by reaction, etc., of a robot arm attached 
to the floating mobile ob j ect , thereby making it possible to control 
the floating mobile object at higher speed with higher precision. 
[0132] Example 3 

The present example corresponds to the above third instance 
of the first embodiment. FIG. 10 illustrates a block diagram of 
a control system 1 according to the present example. A schematic 
configuration of a floating mobile object 10 of the present example 
is as shown in FIG . 5 . 
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The control system 1 according to the present example is 
mainly characterized in that the mechanism of the underwater robot 
is, in an arithmetical manner, divided into a main body part B, 
which receives a thrust, and an effector part E, which generates 
5 athrust, and a virtual thrust trans fer gate G' is virtually provided 
therebetween. 

Note that the virtual thrust transfer gate G' is able to 
estimate all forces and torques applied between the main body part 
B and the effector part E. 

10 [0133] According to the third example also, while additionally 
using a conventional control technique for feeding back a 
position/velocity to avoid influence of drift caused by integration 
error in acceleration, a control system, i.e. , a thrust transfer 
gate system of the present invention, is used to allow a floating 

15 mobile object to stand still in a predetermined position with high 
precision, or track a target trajectory with high precision, even 
under disturbances caused by waves and tidal current or wind or 
disturbances caused by reaction, etc., of a robot arm attached 
to the f loatingmobile object , therebymaking it possible to control 

20 the floating mobile object at higher speed with higher precision. 
[0134] [Implementation by utilizing an inertial navigation 
device] 

Incidentally, inertial navigation devices based on 
existing technology widely used for floating mobile objects such 
25 as airplanes can be used as acceleration sensors , inclination angle 
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sensors and main body part position/velocity sensors, which are 
used in the present invention. 

Specifically, a floating mobile object having an inertial 
navigation device mounted therein can implement a control system 
according to example 3 or the above third instance of the first 
embodiment of the present invention by changing only software 
without adding /changing any hardware component, and therefore it 
is possible to readily realize an increase in speed and precision 
of an existing floating mobile object. 

Todescribe itmore specifically, it is the current situation 
that an existing inertial navigation device measures accelerations 
with sufficiently high precision, but only position/velocity 
information obtained by integrating them is used for control. 
Therefore, by using acceleration information, which has never been 
used as it is, for measuring a thrust within the framework of a 
thrust transfer gate system of the present invention, it is made 
possible to realize an increase in speed and precision compared 
to existing control without changing any conventional hardware 
component . 

Particularly, as for the inertial navigation device, most 
recent inertial navigation devices are of a hybrid type in which 
an acceleration sensor, a gyro, a GPS (Global Positioning System) 
and other sensors are integrated into a unit to obtain acceleration, 
velocity and displacement information, and therefore the measuring 
speed is increased on the whole. Accordingly, the inertial 
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navigation device can be considered as a suitable sensor unit that 
can be used for implementing a control system of the present 
invention. 

[0135] Note that the configuration as described in each of the 
5 above examples is not restrictive, and various modifications in 
design and other variations can be made to the present invention. 

While each of the above examples has been described with 
respect to an underwater robot to which a control system of the 
present invention is applied, the target to which the control system 

10 of the present invention is applied is not limited to the underwater 
robot. As described in the above definition section, in addition 
to the underwater robot , the control system of the present invention 
is applicable to floatingmobile objects in water, air, outer space, 
etc., such as submersibles, helicopters, airships, aircrafts, 

15 spaceships or space robots. In addition, the control system of 
the present invention is applicable to air-cushion vehicles for 
sliding on water or ground, cars and other vehicles with slipping 
wheels, and various mobile objects for sliding on a plane. 

Further, the instances of the above embodiment and the above 

2 0 examples have been described with respect to the case where a control 
system or an acceleration sensing system of the present invention 
is applied to a floating mobile object for floating in 
three-dimensional space. However, when applying the control 
system or acceleration sensing system of the present invention 

25 to a floating mobile object for floating in outer space where 
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basically no gravity is applied or a floating mobile object for 
sliding on a horizontal plane for which it is not necessary to 
consider any inclination or it is possible to ignore any inclination, 
the inclination angle measurement means as defined by the invention 
according to (2) , (3) , (5) or (7) above is not required. In this 
case, by configuring the thrust planning part and the thrust control 
part, such that a gravitational acceleration applied to the main 
body part (gravitational center) of a floating mobile object is 
set to ge = 0, it is made possible to define a control law in a 
more simplified manner. 

In addition, in each of the above instances, sliding mode 
control based on an existing control technique offering robustness 
to a nonlinearity of an effector dynamic characteristic is taken 
as an example of a control law for the thrust control part P3, 
but the control law for the thrust control part P3 is not limited 
to this. For example, as the control low for the thrust control 
part P3, it is possible to apply FID control of input power to 
the thrusters T with respect to thrust error between a target thrust 
at the thrust transfer gate G and a measured or estimated thrust 
at the thrust transfer gate G. 

Further, in each of the above instances, a thrust generation 
device based on propellers dynamically connected to a motor, which 
is a driving source, is used as a thruster T, but the configuration 
of the thruster T is not limited to this. The thruster T may be 
of a jet type and a driving source thereof is not limited to a 
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motor. Various types of driving sources such as an internal 
combustion engine, a steam turbine and a nuclear turbine may be 
used. 

[0136] As described in detail above, it is apparent that the present 
5 invention is a novel and extremely useful invention, which provides 
a system structure capable of allowing a floating mobile object 
to stand still in a predetermined position with high precision, 
or track a target trajectory with high precision, even under 
disturbances caused by waves and tidal current or wind or 
10 disturbances caused by reaction, etc., of a robot arm attached 
to the floating mobile object, thereby making it possible to control 
the floating mobile object at higher speed with higher precision. 



